#pragma warning (disable : 4251 4996 4819 4244)
#include "PlayerProxy.h"
#include <libplayerc++/playerc++.h>
#include <cmath>

PlayerProxy::PlayerProxy(void) : 
m_mapIdToPosProxys(),
m_pClient( new PlayerCc::PlayerClient("localhost") ),
m_pMap( new PlayerCc::MapProxy(m_pClient, 0) )
{
    PopulatePosProxy();

    m_pMap->RequestMap();
}

PlayerProxy::~PlayerProxy(void)
{
}

PlayerProxy& PlayerProxy::Instance()
{
    static PlayerProxy* pInstance = new PlayerProxy();

    return *pInstance;
}

void PlayerProxy::PopulatePosProxy( int nProxyNum )
{
    for ( DevIndexToProxyMap::iterator itLoop = m_mapIdToPosProxys.begin(); m_mapIdToPosProxys.end() != itLoop; ++itLoop )
    {
        delete itLoop->second;
        itLoop->second = NULL;
    }

    m_mapIdToPosProxys.clear();

    PlayerCc::Position2dProxy* pNewProxy = NULL;

    for ( int nDevIndex = 1000; nDevIndex < 1000 + nProxyNum; ++nDevIndex )
    {
        pNewProxy = new PlayerCc::Position2dProxy( m_pClient, nDevIndex );
        m_mapIdToPosProxys.insert( DevIndexToProxyMap::value_type( nDevIndex, pNewProxy ) );
    }

    if ( NULL != m_pClient )
    {
        m_pClient->Read();
    }
}

void PlayerProxy::MoveTo( const int nDevIndex, const double dX, const double dY )
{
    DevIndexToProxyMap::iterator itFound = m_mapIdToPosProxys.find( nDevIndex );
    if ( itFound == m_mapIdToPosProxys.end() )
    {
        return;
    }

    if ( NULL == itFound->second )
    {
        return;
    }

    if ( NULL != m_pClient )
    {
        m_pClient->Read();
    }

    double dOrgX = itFound->second->GetXPos();
    double dOrgY = itFound->second->GetYPos();
    
    double dNewYaw = 0.0;
    if ( abs(dX - dOrgX) > 0.0000001 )
    {
        dNewYaw = std::atan( (dY - dOrgY) / (dX - dOrgX) );
    }
    else
    {
        dNewYaw = 3.1415926 / 2.0;

        if ( dY < dOrgX )
        {
            dNewYaw = -dNewYaw;
        }
    }

    itFound->second->GoTo( dX, dY, dNewYaw );
}

bool PlayerProxy::ArrivedAt( const int nDevIndex, const double dX, const double dY )
{
    DevIndexToProxyMap::iterator itFound = m_mapIdToPosProxys.find( nDevIndex );
    if ( itFound == m_mapIdToPosProxys.end() )
    {
        return true;
    }

    if ( NULL == itFound->second )
    {
        return true;
    }

    if ( NULL != m_pClient )
    {
        m_pClient->Read();
    }

    double dOrgX = itFound->second->GetXPos();
    double dOrgY = itFound->second->GetYPos();

    return (fabs(dX - dOrgX) + fabs(dY - dOrgY)) < 0.05;
}

void PlayerProxy::GetCurPosition( const int nDevIndex, double& dX, double& dY, double& dYaw )
{
    DevIndexToProxyMap::iterator itFound = m_mapIdToPosProxys.find( nDevIndex );
    if ( itFound == m_mapIdToPosProxys.end() )
    {
        return;
    }

    if ( NULL == itFound->second )
    {
        return;
    }

    if ( NULL != m_pClient )
    {
        m_pClient->Read();
    }

    dX = itFound->second->GetXPos();
    dY = itFound->second->GetYPos();
    dYaw = itFound->second->GetYaw();

    std::cout << "Person " << nDevIndex << " Current Position: " << dX << ", " << dY << ", " << dYaw << std::endl;
}

void PlayerProxy::GetCurVelocity( const int nDevIndex, double& dX, double& dY, double& dYaw )
{
    DevIndexToProxyMap::iterator itFound = m_mapIdToPosProxys.find( nDevIndex );
    if ( itFound == m_mapIdToPosProxys.end() )
    {
        return;
    }

    if ( NULL == itFound->second )
    {
        return;
    }

    if ( NULL != m_pClient )
    {
        m_pClient->Read();
    }

    dX = itFound->second->GetXSpeed();
    dY = itFound->second->GetYSpeed();
    dYaw = itFound->second->GetYawSpeed();

    std::cout << "Person " << nDevIndex << " Current Velocity: " << dX << ", " << dY << ", " << dYaw << std::endl;
}

PlayerCc::PlayerClient& PlayerProxy::GetClient()
{
    return *m_pClient;
}

PlayerCc::MapProxy& PlayerProxy::GetMap()
{
    return *m_pMap;
}

void PlayerProxy::RefreshProxy()
{
    if ( NULL != m_pClient )
    {
        m_pClient->Read();
    }
}

void PlayerProxy::GetLatestPosition( const int nDevIndex, double& dX, double& dY, double& dYaw )
{
    DevIndexToProxyMap::iterator itFound = m_mapIdToPosProxys.find( nDevIndex );
    if ( itFound == m_mapIdToPosProxys.end() )
    {
        return;
    }

    if ( NULL == itFound->second )
    {
        return;
    }

    dX = itFound->second->GetXPos();
    dY = itFound->second->GetYPos();
    dYaw = itFound->second->GetYaw();
}

void PlayerProxy::GetLatestVelocity( const int nDevIndex, double& dX, double& dY, double& dYaw )
{
    DevIndexToProxyMap::iterator itFound = m_mapIdToPosProxys.find( nDevIndex );
    if ( itFound == m_mapIdToPosProxys.end() )
    {
        return;
    }

    if ( NULL == itFound->second )
    {
        return;
    }

    dX = itFound->second->GetXSpeed();
    dY = itFound->second->GetYSpeed();
    dYaw = itFound->second->GetYawSpeed();
}
